/*******************************************************************************

  Robot Toolkit ++ (RTK++)

  Copyright (c) 2007-2014 Shuhui Bu <bushuhui@nwpu.edu.cn>
    http://www.adv-ci.com

  ----------------------------------------------------------------------------

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.

*******************************************************************************/

#ifndef __AHRS_MINI__
#define __AHRS_MINI__

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>

#include <deque>

#include "base/osa/osa++.h"
#include "base/types/quaternions.h"
#include "hardware/UART/UART.h"


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

///
/// \struct AHRS_Frame
/// \brief The AHRS_Frame struct
///
/// This struct store estimated Euler angles and raw sensor data
///
struct AHRS_Frame
{
public:
    float       yaw, pitch, roll;           ///< degree
    float       alt, temp, press;           ///< meter, deg, pa
    int         imu_ps;                     ///< IMU per second
    int         crc_a, crcv_a;              ///< CRC received and computed
    uint64_t    tm_a;                       ///< received time (us)

    int         Ax, Ay, Az;                 ///< accelation
    int         Gx, Gy, Gz;                 ///< gyro
    int         Mx, My, Mz;                 ///< megnatic
    int         crc_s, crcv_s;              ///< CRC received and computed
    uint64_t    tm_s;                       ///< received time (us)

    uint64_t    tm;                         ///< received time (us)
    int         correct;                    ///< frame correct or not (1-correct, 0-wrong)

    pi::Quaternions<float>  q;                  ///< quaternary

    float               fA[3];              ///< accelate data (unit: g)
    float               fG[3];              ///< gryo data (unit: degree/sec)
    float               fM[3];              ///< megnatic data

public:
    AHRS_Frame() {
        clear();
    }

    void setEuler(float yaw_, float pitch_, float roll_) {
        yaw   = yaw_;
        pitch = pitch_;
        roll  = roll_;
    }

    void getEuler(float *yaw_, float *pitch_, float *roll_) {
        *yaw_   = yaw;
        *pitch_ = pitch;
        *roll_  = roll;
    }

    void print(void);
    void clear(void);
};


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

///
/// \class AHRS_Mini
/// \brief The AHRS_Mini driver class
///
/// This class manage the AHRS Mini board, read binary data stream from UART and
///     parse them into frames. You can get frame through function 'frame_get'.
///
class AHRS_Mini : public pi::Thread
{
public:
    AHRS_Mini();
    AHRS_Mini(const std::string& port_name);

    ~AHRS_Mini();

    ///
    /// \brief setUART
    /// \param port_name
    /// \return
    ///
    int setUART(const std::string& port_name);

    ///
    /// \brief Begin UART receiving and parsing frame
    /// \return 0  - Success,
    ///         -1 - Can not open UART port
    ///         -2 - Can not start thread
    ///
    int begin(void);

    ///
    /// \brief Stop UART receiving thread
    /// \return 0     - Success
    ///         other - Failed
    ///
    virtual void stop(void);

    ///
    /// \brief Determin frame available or not
    /// \return 0 - no frame, 1 - frame available
    ///
    int frame_ready(void);

    ///
    /// \brief Get current frame number in queue
    /// \return frame number
    ///
    int frame_num(void);

    ///
    /// \brief Clear all frames in the queue
    /// \return 0 - Success
    ///
    int frame_clear(void);

    ///
    /// \brief Get one frame from queue
    /// \return frame
    ///
    AHRS_Frame frame_get(void);

    ///
    /// \brief Get one frame from queue
    /// \param f - AHRS_Frame
    /// \return 0 - Success, 1 - Failed
    ///
    int frame_get(AHRS_Frame &f);

    ///
    /// \brief Get newest frame and clear frame queue
    /// \param f - AHRS_Frame
    /// \return 0 - Success, 1 - Failed
    ///
    int frame_get_last(AHRS_Frame &f);

    ///
    /// \brief Parse AHRS frame from uint8 array
    /// \param buf - uint8_t buffer
    /// \return 0 - Success, 1 - Failed
    ///
    int parse_frame(uint8_t *buf);

    ///
    /// \brief UART receiving thread
    ///
    void threadFunc();

protected:
    pi::UART                uart;               ///< UART object
    int                     isAlive;            ///< Receiving live or not
    AHRS_Frame              frame;              ///< Current frame (for internal usage)

    std::deque<AHRS_Frame>  frameQueue;         ///< Frame queue
    pi::Mutex               mutex;              ///< Frame queue mutex
};

#endif // end of __AHRS_MINI__
